# coding: utf-8

from numpy import array, count_nonzero, vstack, sqrt, logical_or, int64


def merge_point_cloud(source_cloud, target_cloud, max_dist=5):
    """ Brute force method to merge the two point cloud.
    :param source_cloud: the source cloud.
    :param target_cloud: the target cloud.
    :param max_dist: the maximum distance of correspondences.
    :return: the merged cloud.
    """

    def dist_idx(cloud):
        def _wrap(p):
            dist = sqrt(pow(cloud - p, 2).sum(axis=1))
            return [dist.min(), dist.argmin()]
        return _wrap

    source_idx = array(list(map(dist_idx(target_cloud), source_cloud)), dtype=int64)

    # target_idx = array(list(map(dist_idx(source_cloud), target_cloud)), dtype=int64)

    # s_idx = flatnonzero(
    #         logical_or(
    #             source_idx[target_idx[source_idx[:, 1], 1]][:, 1] != source_idx[:, 1],
    #             source_idx[:, 0] > 2
    #         )
    # )

    s_idx = source_idx[:, 0] > max_dist

    if count_nonzero(s_idx) <= 0:
        return target_cloud

    # t_idx = target_idx[:, 0] > max_dist

    # print(">>> appended indexes: %d;" % count_nonzero(s_idx))
    # print(">>> max grid: (%s) (%s);" % (str(source_cloud[s_idx].min(axis=0)), str(source_cloud[s_idx].min(axis=0))))

    return vstack([
        source_cloud[s_idx],
        target_cloud
    ])
